# support revice command to send pwm.for example:#00P1500! or #01P1500!
# 00 is GPIO 7
import time
import 小车.舵机.serial as serial
import RPi.GPIO as GPIO
import atexit
import sys
import os

last_pwm_0=7.5
last_pwm_1=7.5
files=["config_0.txt","config_1.txt"]

def pGpio(postion):
    if postion=='00':
        return 7
    elif postion=='01':
        return 18
    else:
        return 0

def check_data(data):
    if len(data)<9:
        return False
    if data[0] != '#':
        return False
    if pGpio(data[1:3])==0:
        return False
    if data[3] != 'P' and data[3]!='S':
        return False
    if data[8] != '!':
        return False
    command=int(data[4:8])
    if command<500 or command >2500:
        return False
    if data[1:3]=='00':
        if command<1000 or command >2000:
            return False
    elif data[1:3]=="01":
        if command<833 or command>1900:
            return False
    return True

def save_to_file(data):
    try:
        fileName="unknow.txt"
        if data[1:3]=="00":
            fileName=files[0]
        elif data[1:3]=="01":
            fileName=files[1]
        file=open(fileName,'w+')
        file.read()
        file.write(data)
        file.flush()
        file.close()
    finally:
        if file:
            file.close()
    return

def init_pwm_set():
    global last_pwm_0,last_pwm_1
    for file_name in files:
        try:
            if not os.path.exists(file_name):
                continue
            file=open(file_name,'r')
            data=file.readline()
            if check_data(data):
                p_gpio_set=data[1:3]
                pwm_set=pwmWidth(int(data[4:8]))
                if p_gpio_set=='00':
                    last_pwm_0=pwm_set
                elif p_gpio_set=="01":
                    last_pwm_1=pwm_set
            file.close()
        except Exception as e:
            print(e)
            file.close()
    return



def pwmWidth(command):
    return float(format(0.005*command,'.1f'))

def send_pwm(p_gpio,command,last_pwm):
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BOARD)
    print('gpio',p_gpio,command,last_pwm)
    GPIO.setup(p_gpio,GPIO.OUT,initial=GPIO.LOW)
    pwm=GPIO.PWM(p_gpio,50)
    pwm.start(0)
    pwm_width=pwmWidth(command)
    if last_pwm<pwm_width:
        while last_pwm<=pwm_width:
            pwm_current=float(format(last_pwm,'.3f'))
            pwm.ChangeDutyCycle(pwm_current)
            print('pwm',pwm_current)
            last_pwm=float(format(last_pwm+0.005,'.3f'))
            time.sleep(0.005)
        last_pwm=float(format(last_pwm-0.005,'.3f'))
    else:
        while last_pwm>=pwm_width:
            pwm_current=float(format(last_pwm,'.3f'))
            pwm.ChangeDutyCycle(pwm_current)
            print('pwm',pwm_current)
            last_pwm=float(format(last_pwm-0.005,'.3f'))
            time.sleep(0.005)
        last_pwm=float(format(last_pwm+0.005,'.3f'))
    time.sleep(0.02)
    pwm.stop()
    GPIO.cleanup()
    return last_pwm

def deal_result(position,pwm):
    data=int(pwm*200)
    if data<1000:
        return "#"+str(position)+"S0"+str(data)+"!"
    else:
        return "#"+str(position)+"S"+str(data)+"!"

def deal_data(data):
    if not check_data(data):
        return ''
    if data[3]=='S':
        save_to_file(data)
        init_pwm_set()
        return data
    global last_pwm_0,last_pwm_1
    position=data[1:3]
    p_gpio=pGpio(position)
    command=int(data[4:8])
    if position=="00":
        last_pwm_0=send_pwm(p_gpio,command,last_pwm_0)
        save_to_file(deal_result(position=position,pwm=last_pwm_0))
        return data
    elif position=="01":
        last_pwm_1=send_pwm(p_gpio,command,last_pwm_1)
        save_to_file(deal_result(position=position,pwm=last_pwm_1))
        return data
    else:
        # print('unknow p_gpio,please check command,'+data)
        return 'unknow p_gpio,please check command,'+data+"."


def main():
    ser=serial.Serial(port="/dev/ttyAMA0", baudrate=9600,timeout=5)
    if not ser.isOpen():
        ser.open()
    init_pwm_set()
    # ser.write("/dev/ttyAMA0 is open".encode("UTF-8"))
    ser.write(deal_result('00',last_pwm_0).encode("UTF-8"))
    time.sleep(1)
    ser.flushOutput()
    ser.write(deal_result('01',last_pwm_1).encode("UTF-8"))
    atexit.register(GPIO.cleanup)
    try:
        while True:
            size=ser.inWaiting()
            if size==0:
                continue
            time.sleep(0.01)
            size=ser.inWaiting()
            receive=ser.read(size)
            print(receive)
            result=deal_data(receive)
            ser.flushInput()
            if len(result)>0:
                ser.flushOutput()
                ser.write(result.encode("UTF-8"))
                print("result",result)
            time.sleep(0.02)
    except Exception or KeyboardInterrupt as e:
        print('error',e)
        ser.close()

main()